﻿#include <stdexcept>
#include <string>
#include "HerminteCurve.hpp"

namespace MMF
{
    namespace BSpline
    {

        HerminteCurve::HerminteCurve()
            : mFindIdx(0), mLastPos(0.0f)
        {
        }

        void HerminteCurve::SetPointsAndVelocitys(const std::vector<TesVector3> &points, const std::vector<TesVector3> &velocitys)
        {
            // 重置
            Reset();
            mFragments.clear();

            // 点数不够或者点和熟读个数不匹配
            if (points.size() < 2 || points.size() != velocitys.size())
                throw std::length_error((__FILE__ + std::to_string(__LINE__)).c_str());

            // 循环创建子线段
            float curLength = 0.0;
            for (int i = 0; i < points.size() - 1; i++)
            {
                // 获取线段和速度
                std::vector<TesVector3> subPoints(2);
                std::vector<TesVector3> subVelocitys(2);
                subPoints[0] = points[i];
                subPoints[1] = points[i + 1];
                subVelocitys[0] = velocitys[i];
                subVelocitys[1] = velocitys[i + 1];

                // 计算长度
                float length = (subPoints[1] - subPoints[0]).Length();

                // 创建子线段
                HermiteFrag frag(curLength, subPoints, subVelocitys);
                curLength += length;

                // 放入数据
                mFragments.push_back(frag);
            }
        }

        TesVector3 HerminteCurve::GetPoint(float distance)
        {
            // 如果是正向找点
            if (distance >= mFragments[mFindIdx].mStartPos)
            {
                // 向前找到正确的线段
                while (!mFragments[mFindIdx].isIn(distance))
                    mFindIdx++;
            }
            else
            {
                // 反向找点
                while (!mFragments[mFindIdx].isIn(distance))
                    mFindIdx--;
            }

            // 获取插值
            return mFragments[mFindIdx].GetPoint(distance);
        }

        bool HerminteCurve::IsEnd()
        {
            if (mFindIdx == mFragments.size() - 1 && mLastPos >= mFragments.back().mStartPos + mFragments.back().mLength)
                return true;

            return false;
        }

        float HerminteCurve::Length()
        {
            return mFragments.back().mStartPos + mFragments.back().mLength;
        }

        void HerminteCurve::Reset()
        {
            mFindIdx = 0;
            mLastPos = 0.0f;
        }

    }
}